package FRC2856.Robot;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Victor;
public class Capture {

    //Array to be returned by the "check" method.
    //Numbers are:
    //   FIRST DOUBLE:  1 or 0 for TRUE or FALSE state of lower digital IO
    //   SECOND DOUBLE: 1 or 0 for TRUE or FALSE state of upper digital IO
    //   THIRD DOUBLE:  double ranging from -1 to +1 for speed of lower motor
    //   FOURTH DOUBLE: double ranging from -1 to +1 for speed of upper motor
    double[] checker = new double[4];

    //Number of balls held
    int numBalls;

    //Declare victors
    Victor lowerVic;
    Victor upperVic;

    //Declare Digital IOs
    DigitalInput lowerIO;
    DigitalInput upperIO;
    DigitalInput middleIO;
    boolean maxPowerBool = false;
    double[] temporaryStorage = new double[0];
    boolean[] sensors = new boolean[3];
    
    
    boolean middleTripped, lowerTripped, upperTripped;
    //Constructor, victors and IOs created here
    public Capture(){
        //Victor controllers for motors
     //   lowerVic = new Victor(1, 4);
     //   upperVic = new Victor(1, 5);

        middleTripped=false;
        lowerTripped=false;
        upperTripped=false;
        
        lowerVic= new Victor(RobotMap.captureLowerVictorPort, RobotMap.captureLowerVictorChannel);
        upperVic= new Victor(RobotMap.captureUpperVictorPort, RobotMap.captureUpperVictorChannel);

        //Digital IOs
//        lowerIO = new DigitalInput(RobotMap.infraredLowerPort, RobotMap.infraredLowerChannel);
//        upperIO = new DigitalInput(RobotMap.infraredUpperPort, RobotMap.infraredUpperChannel);
//        middleIO = new DigitalInput(RobotMap.infraredMiddlePort, RobotMap.infraredMiddleChannel);
          lowerIO = new DigitalInput(RobotMap.capSensorModule, RobotMap.capLowerSensorChannel);
          middleIO = new DigitalInput(RobotMap.capSensorModule, RobotMap.capMiddleSensorChannel);
          upperIO = new DigitalInput(RobotMap.capSensorModule, RobotMap.capUpperSensorChannel);

//        lowerVic.set(1.0);
//        upperVic.set(1.0);
    }

    //getStatus method, returns checker array with important values
    public double[] getStatus(){
  /*      if (lowerIO.get()==true)
            checker[0]=1.0;
        if (lowerIO.get()==false)
            checker[0]=0.0;
        if (upperIO.get()==true)
            checker[1]=1.0;
        if (upperIO.get()==false)
            checker[1]=0.0;
        checker[2]=lowerVic.get();
        checker[3]=upperVic.get();
*/
        return checker;
    }


    public boolean[] getSensorStatus(){
        sensors[0]=lowerIO.get();
        sensors[1]=middleIO.get();
        sensors[2]=upperIO.get();
        
        return sensors;
    }
    //Check method, suggested to be called every teleoperated periodic
    //May not provide accurate checking if aaron's toilet seat is a picture of a program
    public void check(){
/*        if (lowerIO.get()==true){
            if(!lowerTripped){
                lowerTripped=true;
            }
        }
        if (upperIO.get()==true){
            if(!upperTripped){
                upperTripped=true;
            }
        }
        if (middleIO.get()==true){
            if(!middleTripped){
                middleTripped=true;
            }
        }
        if (lowerIO.get()==false){
            if(lowerTripped){
                lowerTripped=false;
                numBalls++;
            }
        }
        if (upperIO.get()==false){
            if(upperTripped){
                upperTripped=false;
                numBalls--;
            }
        }
        if (middleIO.get()==false){
            if(middleTripped){
                middleTripped=false;
            }
        }
*/        
    }

    public void maxPower(boolean on){
        if(on){
            temporaryStorage = getStatus();
            maxPowerBool = true;
            lowerVic.set(0);
            upperVic.set(0);
        }
        else{
            maxPowerBool = false;
            lowerVic.set(temporaryStorage[2]);
            upperVic.set(temporaryStorage[3]);
        }
        
    }


    //Set speed of lower motor
    public void setLowerSpeed(double x){
        if(x == 0 || !maxPowerBool)
            lowerVic.set(x);
    }

    //Set speed of upper motor
    public void setUpperSpeed(double y){
        if(y == 0 || !maxPowerBool)
            upperVic.set(y);
    }

    //Get speed of lower motor
    public double getLowerSpeed(){
        return lowerVic.get();
    }

    //Get speed of upper motor
    public double getUpperSpeed(){
        return upperVic.get();
    }

    //Get state of lower Digital IO
    public boolean getLowerIO(){
//        return lowerIO.get();
        return false;
    }

    //Get state of upper Digital IO
    public boolean getUpperIO(){
//        return upperIO.get();
        return false;
    }

    public void turnAllOff(){
        setLowerSpeed(0);
        setUpperSpeed(0);
    }
    public void turnAllOn(){
        setLowerSpeed(1);
        setUpperSpeed(1);
    }

    public void analogToDigital(){

    }



}

